#include "CalibrationControl.h"
#include <iostream>

float glbMouseX = -1.0f;
float glbMouseY = -1.0f;

CalibrationControl::CalibrationControl ()
  : GlControl (), _checkPoint (2),
  _cameraPoint (UNDISTORT_NUM, 2), _projectPoint (UNDISTORT_NUM, 2)
{
  int width = 0, height = 0;
  _get_terrain_size (width, height);

  _pShowListMng = new ShowListManager ();
  _pSceneCoordSpaceInfo = new SceneCoordSpaceInfo (width, height);
  _pStringControl = new myglStringControl (L"Forte", 20, _pShowListMng);
   _pLightInput = new LightInputDriver (NULL, NULL, NULL);

  _step = INITIAL;
  _pointCollected = 0;
  _calibrationPointCurrentIndex = 0;

  _checkPoint[X] = -1.0f;
  _checkPoint[Y] = -1.0f;

  _calibrationPointIndexArr[0] = 0;
  _calibrationPointIndexArr[1] = 2;
  _calibrationPointIndexArr[2] = 4;
  _calibrationPointIndexArr[3] = 6;
  _calibrationPointIndexArr[4] = 8;
}

void CalibrationControl::init_render ()
{

  glEnable(GL_DEPTH_TEST);
  glEnable(GL_COLOR_MATERIAL);
  glEnable (GL_BLEND);
  glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  glShadeModel(GL_SMOOTH);

  glEnable(GL_LINE_SMOOTH);
  glHint(GL_LINE_SMOOTH_HINT,GL_NICEST);
  glEnable(GL_POLYGON_SMOOTH);
  glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST);
  
  glClearColor (0.0f, 0.0f, 0.0f, 1.0f);

}

void CalibrationControl::draw ()
{
  switch (_step)
  {
  case INITIAL:
    glTranslatef (0.0f, 0.0f, -2.0f);
    glColor3f (1.0f, 1.0f, 1.0f);
    _pStringControl->DrawString ("Press Enter to start...", 0.1f);
    break;
  case ADJUST_POSITION:
    _pSceneCoordSpaceInfo->show_border ();
    glTranslatef (0.0f, 1.0f, -3.0f);
    glColor3f (1.0f, 1.0f, 1.0f);
    _pStringControl->DrawString (
      "Adjust Key: Up Down Left Right Z X\nPress Enter to go to next step...",
      0.1f);
    break;
  case CALIBRATION:
    _pSceneCoordSpaceInfo->show_border ();
    _pSceneCoordSpaceInfo->show_calibration_point (
      _calibrationPointIndexArr[_calibrationPointCurrentIndex],
      _projectPoint[_calibrationPointCurrentIndex]);
    _pSceneCoordSpaceInfo->show_calibration_process (
      _calibrationPointIndexArr[_calibrationPointCurrentIndex]
      , float (_pointCollected) / float (DATA_NEED_PER_POINT));
    break;
  case CHECK:
    _pSceneCoordSpaceInfo->show_border ();
    if (_checkPoint[X] > 0 && _checkPoint[Y] > 0)
      _pSceneCoordSpaceInfo->show_point (_checkPoint[X], _checkPoint[Y]);
    break;
  case SAVE_DATA:
    glTranslatef (0.0f, 0.0f, -2.0f);
    glColor3f (1.0f, 1.0f, 1.0f);
    _pStringControl->DrawString ("Saving data...", 0.1f);
    break;
  case COMPLETE:
    glTranslatef (0.0f, 0.0f, -2.0f);
    glColor3f (1.0f, 1.0f, 1.0f);
    _pStringControl->DrawString ("Configure Complete!\nPress Enter to exit.", 0.1f);
    break;
  }
}

void CalibrationControl::handle_keypress (unsigned char key, int x, int y)
{
  switch (key)
  {
  case 13:
    if (_step != CALIBRATION && _step != SAVE_DATA)
      _go_next_step ();
    break;
  case 27:
    exit (0);
    break;
  case 'z':
    if (_step == ADJUST_POSITION)
      _pSceneCoordSpaceInfo->adjust_pos (0.0f, 0.0f, -5.0f);
	 
    break;
  case 'x':
    if (_step == ADJUST_POSITION)
      _pSceneCoordSpaceInfo->adjust_pos (0.0f, 0.0f, 5.0f);
    break;
  }
}

void CalibrationControl::handle_special_keypress (int key, int x, int y)
{
  if (_step == ADJUST_POSITION)
  {
    switch (key)
    {
    case GLUT_KEY_LEFT:
       _pSceneCoordSpaceInfo->adjust_angle (1.0f, 0.0f, 0.0f);
      break;
    case GLUT_KEY_RIGHT:
      _pSceneCoordSpaceInfo->adjust_angle (-1.0f, 0.0f, 0.0f);
    }
  }
}



int CalibrationControl::handle_timer (int val)
{
  switch (_step)
  {
  case CALIBRATION:
    {
      Array< float > cP;

      if (_pLightInput->get_point_in_camera (cP))
        _add_camera_point_data (cP);
      else
        _pointCollected = 0;

      if (_pointCollected == DATA_NEED_PER_POINT)
      {
        ++_calibrationPointCurrentIndex;
        _pointCollected = 0;
      }
      if (_calibrationPointCurrentIndex == UNDISTORT_NUM)
      {
        if (!_pLightInput->config (_cameraPoint, _projectPoint));
        _go_next_step ();
      }
    }
    break;
  case CHECK:
    if (!_pLightInput->get_point_in_project (_checkPoint))
    {
      _checkPoint[X] = -1.0f;
      _checkPoint[Y] = -1.0f;
    }
    break;
  case SAVE_DATA:
    _pLightInput->write_file ();

    // Write scene coord space info
    FileStreamOut fout (SCENE_COORD_DATA_FILE);
    fout.bin_write< char > (DATA_FILE_TYPE, 2);
    fout << (*_pSceneCoordSpaceInfo);
    fout.close ();

    _go_next_step ();

    break;
  }
  return 0;
}



// ========================================================================
// Private functions
// ========================================================================

bool CalibrationControl::_get_terrain_size (int& width, int& height)
{

  ULONG_PTR gdipToken;
  GdiplusStartupInput gdipSIn;

  GdiplusStartup (&gdipToken, &gdipSIn, NULL);

  // Load bitmap
  Ptr< Bitmap > bitmap = new Bitmap (TERRAIN_FILE);

  width = bitmap->GetWidth () - 1;
  height = bitmap->GetHeight () - 1;

  bitmap = NULL;

  GdiplusShutdown (gdipToken);

  return true;

}



bool CalibrationControl::_add_camera_point_data (const Array< float >& newPoint)
{
  if (newPoint[X] < 0 || newPoint[Y] < 0)
    return false;

  if (_pointCollected == 0)
  {
    _cameraPoint[_calibrationPointCurrentIndex][0] = newPoint[X];
    _cameraPoint[_calibrationPointCurrentIndex][1] = newPoint[Y];
  }
  else if (std::abs (_cameraPoint[_calibrationPointCurrentIndex][0] - newPoint[X]) > 3
    || std::abs (_cameraPoint[_calibrationPointCurrentIndex][1] - newPoint[Y]) > 3)
    return false;

  _cameraPoint[_calibrationPointCurrentIndex][0]
    = (_cameraPoint[_calibrationPointCurrentIndex][0] * _pointCollected + newPoint[X]) / (_pointCollected + 1);
  _cameraPoint[_calibrationPointCurrentIndex][1]
    = (_cameraPoint[_calibrationPointCurrentIndex][1] * _pointCollected + newPoint[Y]) / (_pointCollected + 1);
  ++_pointCollected;

  return true;
}

void CalibrationControl::handle_mouse (int button, int state, int x, int y)
{
  if (button == GLUT_LEFT_BUTTON  && state == GLUT_DOWN)
  {
    glbMouseX = x;
    glbMouseY = y;
  }
  else
  {
    glbMouseX = -1.0f;
    glbMouseY = -1.0f;
  }
}